import cv2
import numpy as np
import matplotlib.pyplot as plt
import glob

'''
作者：SXL；
日期：2022.9.15；
功能：对比原始参数和优化参数识别标定板角点间距的精度；
输入：双目相机原始内外参数（mtx_l、mtx_r、R_lr、T_lr）和优化后的内外参数（mtx_ly、mtx_ry、R_lry、T_lry）;
输出：原始相机参数识别的棋盘格角点间距和优化参数识别的棋盘格角点间距。
'''
# 原始数据的左相机内参
mtx_l = np.array([[1.55545032e+03 ,0.00000000e+00, 6.62505387e+02,0],
                 [0.00000000e+00 ,1.55546765e+03, 5.37266926e+02,0],
                 [0.00000000e+00, 0.00000000e+00 ,1.00000000e+00,0]])
# 原始数据的右相机内参
mtx_r = np.array([[1.55492456e+03 ,0.00000000e+00 ,6.17343953e+02,0],
                 [0.00000000e+00 ,1.55441833e+03 ,4.81399857e+02,0],
                 [0.00000000e+00, 0.00000000e+00, 1.00000000e+00,0]])
# 原始数据的右相机到左相机旋转矩阵
R_lr = np.array([[0.999892485721600,-0.00134230600479356,0.0146018907018903],
                 [0.00123473877089462,0.999972055373835,0.00737318734302526],
                 [-0.0146113797311583,-0.00735436509953058,0.999866201497148]])
# 原始数据的右相机到左相机平移矩阵
T_lr = np.array([[-138.421087392432],
                 [0.0340411468217300],
                 [0.924604888200920]])

# 优化数据的左相机内参
mtx_ly = np.array([[1.55933174e+03 , 0.00000000e+00 ,6.59779625e+02,0],
                   [0.00000000e+00 ,1.55932600e+03,5.32380324e+02,0],
                   [0.00000000e+00 ,0.00000000e+00, 1.00000000e+00,0]])
# 优化数据的右相机内参
mtx_ry = np.array( [[ 1.56191731e+03,0.00000000e+00 ,6.15497708e+02 ,0],
                    [0.00000000e+00,1.56135500e+03,4.76718718e+02,0],
                    [0.00000000e+00, 0.00000000e+00, 1.00000000e+00,0]])
# 优化数据的右相机到左相机旋转矩阵
R_lry = np.array([[ 9.99575962e-01 ,-1.36307186e-03 , 1.45749359e-02 ],
                  [1.21047140e-03,9.99700766e-01  ,7.36263200e-03 ],
                  [-1.43474342e-02 ,-6.71413977e-03,1.00049944e+00]])
# 优化数据的右相机到左相机平移矩阵
T_lry = np.array([[-1.38315256e+02],
                  [ -1.09815195e-01],
                  [ 1.97777804e+00]])

cameraMatrixL=mtx_l[:,0:3]
cameraMatrixR=mtx_r[:,0:3]
cameraMatrixLy=mtx_ly[:,0:3]
cameraMatrixRy=mtx_ry[:,0:3]

#左相机到左相机的投影矩阵
R_ll = ([[1,0,0],
         [0,1,0],
         [0,0,1]])
T_ll = ([[0],[0],[0]])
temp_R_ll = np.append(R_ll, T_ll, axis =1)
_temp_R_ll = np.row_stack((temp_R_ll, [0,0,0,1]))
P0 = np.dot(mtx_l, _temp_R_ll)
P0y = np.dot(mtx_ly, _temp_R_ll)

#左相机到右相机的投影矩阵
temp_R_lr = np.append(R_lr, T_lr, axis =1)
_temp_R_lr = np.row_stack((temp_R_lr, [0,0,0,1]))
P1 = np.dot(mtx_r, _temp_R_lr)
temp_R_lry = np.append(R_lry, T_lry, axis =1)
_temp_R_lry = np.row_stack((temp_R_lry, [0,0,0,1]))
P1y = np.dot(mtx_ry, _temp_R_lry)

# 读取图片
imagesL = glob.glob('ALL\\*.bmp')
imagesR = glob.glob('ARR\\*.bmp')

for imgL in imagesL:
    grayImageL = cv2.imread(imgL,0)
    imgR = imgL.replace('ALL','ARR')
    imgR = imgR.replace('LL','RR')
    grayImageR = cv2.imread(imgR,0)

    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    retLO, cornersLO = cv2.findChessboardCorners(grayImageL, (15, 11), None)
    retRO, cornersRO = cv2.findChessboardCorners(grayImageR, (15, 11), None)
    cornersLO = cv2.cornerSubPix(grayImageL, cornersLO,(15, 11), (-1, -1), criteria)
    cornersRO = cv2.cornerSubPix(grayImageR, cornersRO,(15, 11), (-1, -1), criteria)

    i = 0
    # 计算棋盘格横向角点坐标
    wO_distance = []
    wOy_distance = []
    for i in range(cornersLO.shape[0]):
        if (i+1)%15 == 0:
            continue

        lO0 = np.array([cornersLO[i][0][0], cornersLO[i][0][1]])
        rO0 = np.array([cornersRO[i][0][0], cornersRO[i][0][1]])
        sO0 = np.array(cv2.triangulatePoints(P0, P1,lO0,rO0)).T
        wO0 = sO0[0][:-1]/np.max(sO0[0][-1])
        sO0y = np.array(cv2.triangulatePoints(P0y, P1y,lO0,rO0)).T
        wO0y=sO0y[0][:-1]/np.max(sO0y[0][-1])

        lO1 = np.array([cornersLO[i+1][0][0], cornersLO[i+1][0][1]])
        rO1 = np.array([cornersRO[i+1][0][0], cornersRO[i+1][0][1]])
        sO1 = np.array(cv2.triangulatePoints(P0, P1,lO1,rO1)).T
        wO1 = sO1[0][:-1]/np.max(sO1[0][-1])
        sO1y = np.array(cv2.triangulatePoints(P0y, P1y,lO1,rO1)).T
        wO1y=sO1y[0][:-1]/np.max(sO1y[0][-1])

        distance = np.sqrt((wO0[0] - wO1[0]) ** 2 + (wO0[1] - wO1[1]) ** 2 + (wO0[2] - wO1[2]) ** 2)
        distancey = np.sqrt((wO0y[0] - wO1y[0]) ** 2 + (wO0y[1] - wO1y[1]) ** 2 + (wO0y[2] - wO1y[2]) ** 2)
        wO_distance.append(distance)
        wOy_distance.append(distancey)

    # 计算棋盘格纵向角点坐标
    hOy_distance =[]
    hO_distance = []
    for i in range(0, 15):
        for j in range(i, len(cornersRO),15):
            if j >= 150:
                continue
            lO0 = np.array([cornersLO[j][0][0], cornersLO[j][0][1]])
            rO0 = np.array([cornersRO[j][0][0], cornersRO[j][0][1]])
            sO0 = np.array(cv2.triangulatePoints(P0, P1, lO0, rO0)).T
            wO0 = sO0[0][:-1] / np.max(sO0[0][-1])
            sO0y = np.array(cv2.triangulatePoints(P0y, P1y, lO0, rO0)).T
            wO0y = sO0y[0][:-1] / np.max(sO0y[0][-1])

            lO1 = np.array([cornersLO[j + 15][0][0], cornersLO[j + 15][0][1]])
            rO1 = np.array([cornersRO[j + 15][0][0], cornersRO[j + 15][0][1]])
            sO1 = np.array(cv2.triangulatePoints(P0, P1, lO1, rO1)).T
            wO1 = sO1[0][:-1] / np.max(sO1[0][-1])
            sO1y = np.array(cv2.triangulatePoints(P0y, P1y, lO1, rO1)).T
            wO1y = sO1y[0][:-1] / np.max(sO1y[0][-1])

            distance = np.sqrt((wO0[0] - wO1[0]) ** 2 + (wO0[1] - wO1[1]) ** 2 + (wO0[2] - wO1[2]) ** 2)
            distancey = np.sqrt((wO0y[0] - wO1y[0]) ** 2 + (wO0y[1] - wO1y[1]) ** 2 + (wO0y[2] - wO1y[2]) ** 2)
            hO_distance.append(distance)
            hOy_distance.append(distancey)

    distance=np.append(wOy_distance,hOy_distance)
    distanceO=np.append(wO_distance,hO_distance)
    PPM=(max(distance)-min(distance))
    PPMO=(max(distanceO)-min(distanceO))

    plt.figure(1)
    plt.title('Error value of calculation')
    plt.xlabel("The number of feature points (pcx)")
    plt.ylabel("Corners distance (mm)")
    plt.plot(distance, color='red', label=['Optimize parameters',"%.3f"%float(PPM)])
    plt.plot(distanceO, color='gray', label=['Initial value', "%.3f" % float(PPMO)])
    plt.legend(loc='best')
    plt.show()

    print('初值',PPMO)
    print('优化参数',PPM)




